import numpy as np
from utility import *

class RefPoint:
    def __init__(self,refline,xmin,xmax):
        self.refline_table={"x":[],"y":[],"s":[]}
        self.refline=refline
        self.xmin=xmin
        self.xmax=xmax
    def run(self):
        for x in np.linspace(self.xmin,self.xmax,100):
            self.refline_table["x"].append(x)
            t_y=self.refline.getRefLine()(x)
            self.refline_table["y"].append(t_y)
            if len(self.refline_table["s"])==0:
                self.refline_table["s"].append(0)
            else:
                xk1=self.refline_table["x"][-1]
                yk1=self.refline_table["y"][-1]
                sk1=self.refline_table["s"][-1]
                t_s=calcDist([x,t_y],[xk1,yk1])+sk1
                self.refline_table["s"].append(t_s)

    def getCloestPoint(self,ego_pose):
        temp=999
        cloest_index=0
        for index in range(len(self.refline_table["x"])):
            t_x=self.refline_table["x"][index]
            t_y=self.refline_table["y"][index]
            t_dis=calcDist(ego_pose,[t_x,t_y])
            if t_dis<temp:
                temp=t_dis
                cloest_index=index
            elif t_y>=1.5*ego_pose[0]:
                break
        rs=self.refline_table["s"][cloest_index]
        rx=self.refline_table["x"][cloest_index]
        ry=self.refline_table["y"][cloest_index]
        rtheta=np.polyder(self.refline)(rx)
        rkappa=np.polyder(self.refline,2)(rx)
        rdkappa=np.polyder(self.refline_table,3)(rx)
        return [rs,rx,ry,rtheta,rkappa,rdkappa]

    def getRefTable(self):
        return self.refline_table